#include <iostream>
using namespace std;
#include <stdio.h>
#include <cmath>

#include <mex.h>
#include "mexFileIO.h"

#include <vector>
#include <algorithm>
#include <queue>
#include <limits>
#include <map>
using namespace std;
#include <mexFileIO.h>
#include <szMexUtility.h>
#include <szMexUtilityTemplate.h>
#include <szMyNeighborOp.h>
#include <szMiscOperations.h>
#include <szConvexHull2D.h>
#include <ContourEQW.h>
#include <szParticleF.h>
#include <TriangulationWithMemento.h>
#include <FragmentInfo.h>
//#include <TriangulationHelper.h>
#include <IntersectionConvexPolygons.h>
#include <GraphFactory.h>
typedef CParticleF graphKey;

GraphFactory<graphKey>* GraphFactory<graphKey>::_instance = NULL;

struct MovingParticle
{
	MovingParticle(){}
	MovingParticle(CParticleF& p, CParticleF& v)
	{
		this->p = p;
		this->v = v;
	}
	CParticleF move(float t)
	{
		float x = p.m_X + t * v.m_X;
		float y = p.m_Y + t * v.m_Y;
		return CParticleF(x, y);
	}
	CParticleF p;
	CParticleF v;
};

bool
coLinear(CParticleF& a, CParticleF& b, CParticleF& c, float precision = 1.0e-3)
{
	float d = Distance2Line(a, c, b);
	return d < precision;
}

vector<CParticleF>
removeDegenerate(vector<CParticleF> points)
{
	while(true)
	{
		vector<CParticleF> result;
		bool bChanged = false;
		for(int i=0; i<points.size(); ++i)
		{
			int i0=(i-1+points.size()) % points.size();
			int i2=(i+1) % points.size();
			if(coLinear(points[i0], points[i], points[i2]))
			{
				bChanged = true;
			}
			else
			{
				result.push_back(points[i]);
			}
		}
		if(bChanged == false)
		{
			return result;
		}
		else
		{
			points = result;
		}
	}
}

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{   
	printf("%s: This build was compiled at %s %s\n", "StraightMedialAxis", __DATE__, __TIME__);
	if (nrhs < 1 || nlhs < 0)
	{
		mexErrMsgTxt("Usage: [X Y] = StraightMedialAxis(P, [iter delta])");
		return;
	}
	//Points
	vector<CParticleF> points;
	const int* dimsP;
	{
		vector<float> P0;
		mxClassID classIdP;
		int ndimP;
		LoadData(P0, prhs[0], classIdP, ndimP, &dimsP);
		for(int i=0; i<dimsP[0]; ++i)
		{
			float x = GetData2(P0, i, 0, dimsP[0], dimsP[1], (float)0);
			float y = GetData2(P0, i, 1, dimsP[0], dimsP[1], (float)0);
			points.push_back(CParticleF(x, y));
		}
	}
	float tmin0 = std::numeric_limits<float>::infinity();
	if(nrhs >= 2)
	{
		mxClassID classMode;
		ReadScalar(tmin0,prhs[1],classMode);
	} 

	points = removeDegenerate(points);

	vector<MovingParticle> v;
	for(int i=0; i<points.size(); ++i)
	{
		int i0=(i-1+points.size()) % points.size();
		int i2=(i+1) % points.size();
		float x0 = points[i0].m_X - points[i].m_X;		
		float y0 = points[i0].m_Y - points[i].m_Y;
		float len0 = sqrt(x0*x0 + y0*y0);
		x0 /= len0;
		y0 /= len0;
		float x2 = points[i2].m_X - points[i].m_X;		
		float y2 = points[i2].m_Y - points[i].m_Y;
		float len2 = sqrt(x2*x2 + y2*y2);
		x2 /= len2;
		y2 /= len2;
		float vx = (x0 + x2)/2.0;
		float vy = (y0 + y2)/2.0;
		CParticleF m(points[i].m_X + vx, points[i].m_Y + vy);
		if(inside(m, points) == false)
		{
			printf("%d is outside.\n", i+1);
			vx = -vx;
			vy = -vy;
		}
		float len = sqrt(vx*vx + vy*vy);
		if(Abs(len) > 1.0e-6)
		{
			vx /= len;
			vy /= len;
		}
		else
		{
			vx = 0;
			vy = 0;
		}
		v.push_back(MovingParticle(points[i], CParticleF(vx, vy)));
	}

	float tmin = std::numeric_limits<float>::infinity();
	for(int i=0; i<v.size(); ++i)
	{
		for(int j=i+1; j<v.size(); ++j)
		{
			pair<float,float> t = _IntersectConvexPolygon::intersect(v[i].p, v[i].move(1.0f), v[j].p, v[j].move(1.0f));
			if(t.first != t.first || t.first <= 0 || t.second <= 0) continue;
			CParticleF a = v[i].move(t.first);
			CParticleF b = v[j].move(t.second);
			//printf("%d (%1.1f,%1.1f), %d (%1.1f,%1.1f) intersects at (%1.1f,%1.1f) or (%1.1f,%1.1f) %3.3f, %3.3f.\n",
			//	i+1, v[i].p.m_X, v[i].p.m_Y, j+1, v[j].p.m_X, v[j].p.m_Y, a.m_X, a.m_Y, b.m_X, b.m_Y, t.first, t.second);
			tmin = Min(tmin, Max(t.first, t.second));
		}
	}
	printf("tmin = %f\n", tmin);
	tmin = Min(tmin, tmin0);

	if(nlhs >= 1)
	{
		const int dims[] = {points.size(), 4};
		vector<float> F(dims[0]*dims[1]);
		for(int i=0; i<dims[0]; ++i)
		{
			SetData2(F, i, 0, dims[0], dims[1], v[i].p.m_X);
			SetData2(F, i, 1, dims[0], dims[1], v[i].p.m_Y);
			CParticleF q = v[i].move(tmin);
			SetData2(F, i, 2, dims[0], dims[1], q.m_X);
			SetData2(F, i, 3, dims[0], dims[1], q.m_Y);
		}
		plhs[0] = StoreData(F, mxSINGLE_CLASS, 2, dims);
	}
	if(nlhs >= 2)
	{
		const int dims[] = {v.size(), 4};
		vector<float> F(dims[0]*dims[1]);
		for(int i=0; i<dims[0]; ++i)
		{
			SetData2(F, i, 0, dims[0], dims[1], v[i].p.m_X);
			SetData2(F, i, 1, dims[0], dims[1], v[i].p.m_Y);
			SetData2(F, i, 2, dims[0], dims[1], v[i].v.m_X);
			SetData2(F, i, 3, dims[0], dims[1], v[i].v.m_Y);
		}
		plhs[1] = StoreData(F, mxSINGLE_CLASS, 2, dims);
	}

	mexUnlock();
}

